#include "car_init.hpp"

#ifndef ZDT_MOTOR_USE_CONFIG_H
#define ZDT_MOTOR_RS485_PATH_12 ("/dev/ttyS6")
#define ZDT_MOTOR_RS485_BAUDRATE_12 (115200)
#define ZDT_MOTOR_RS485_PATH_34 ("/dev/ttyS9")
#define ZDT_MOTOR_RS485_BAUDRATE_34 (115200)
#define ZDT_MOTOR_RS485_PATH_5678 ("/dev/ttyS7") // s1
#define ZDT_MOTOR_RS485_BAUDRATE_5678 (115200)

#define TJC_SCREEN_PATH ("/dev/ttyS4")
#define TJC_SCREEN_BAUDRATE (115200)

#define CODE_SCAN_BAUDRATE (115200)

#define ZDT_MOTOR_MODBUS_SLAVE_ADDR (0x01)
#define ZDT_MOTOR_RS485_FRAME_TIMEOUT_MS (100)

#endif // ZDT_MOTOR_USE_CONFIG_H

uart_v2 *Pmodbus_zdt_uart_12 = NULL;
rs485_v3 *Pmodbus_zdt_rs485_12 = NULL;
uart_v2 *Pmodbus_zdt_uart_34 = NULL;
rs485_v3 *Pmodbus_zdt_rs485_34 = NULL;
uart_v2 *Pmodbus_zdt_uart_5678 = NULL;
rs485_v3 *Pmodbus_zdt_rs485_5678 = NULL;

agile_modbus_rtu_master_simple *Pmodbus_zdt_rtu_master_12 = NULL;
agile_modbus_rtu_master_simple *Pmodbus_zdt_rtu_master_34 = NULL;
agile_modbus_rtu_master_simple *Pmodbus_zdt_rtu_master_5678 = NULL;

zdt_motor *Pmodbus_wheel_1 = NULL;
zdt_motor *Pmodbus_wheel_2 = NULL;
zdt_motor *Pmodbus_wheel_3 = NULL;
zdt_motor *Pmodbus_wheel_4 = NULL;

zdt_motor *Pmodbus_rotate_5 = NULL;
zdt_motor *Pmodbus_lift_6 = NULL;
zdt_motor *Pmodbus_bracket_7 = NULL;

pwm_board *Pmodbus_claw_board_8 = NULL;

i2c_v1 *hwt101_i2c = nullptr;
hwt101 *hwt101_dev = nullptr;

spi_v1 *pmw3901_spi = nullptr;
pmw3901 *pmw3901_dev = nullptr;

uart_v2 *screen_uart = nullptr;
tjc_screen *screen = nullptr;

uart_v2 *scanner_uart = nullptr;
code_scan *scanner = nullptr;

car *Pcar = NULL;
yj_return car_all_init_flag = YJ_ERROR;

std::thread screen_fresh;
void car_sensor_deinit();

int modbus_zdt_rs485_modbus_read_12(uint8_t *data)
{
    if (!Pmodbus_zdt_rs485_12 || !Pmodbus_zdt_uart_12 || !Pmodbus_zdt_rtu_master_12 || !data)
        return -1;
    return Pmodbus_zdt_rs485_12->rs485_recv(data, MAX_ADU_LENGTH);
}

int modbus_zdt_rs485_modbus_write_12(uint8_t *data, uint32_t len)
{
    if (!Pmodbus_zdt_rs485_12 || !Pmodbus_zdt_uart_12 || !Pmodbus_zdt_rtu_master_12 || !data)
        return -1;
    return Pmodbus_zdt_rs485_12->rs485_send(data, len);
}
int modbus_zdt_rs485_modbus_read_34(uint8_t *data)
{
    if (!Pmodbus_zdt_rs485_34 || !Pmodbus_zdt_uart_34 || !Pmodbus_zdt_rtu_master_34 || !data)
        return -1;
    return Pmodbus_zdt_rs485_34->rs485_recv(data, MAX_ADU_LENGTH);
}

int modbus_zdt_rs485_modbus_write_34(uint8_t *data, uint32_t len)
{
    if (!Pmodbus_zdt_rs485_34 || !Pmodbus_zdt_uart_34 || !Pmodbus_zdt_rtu_master_34 || !data)
        return -1;
    return Pmodbus_zdt_rs485_34->rs485_send(data, len);
}

int modbus_zdt_rs485_modbus_read_5678(uint8_t *data)
{
    if (!Pmodbus_zdt_rs485_5678 || !Pmodbus_zdt_uart_5678 || !Pmodbus_zdt_rtu_master_5678 || !data)
        return -1;
    return Pmodbus_zdt_rs485_5678->rs485_recv(data, MAX_ADU_LENGTH);
}

int modbus_zdt_rs485_modbus_write_5678(uint8_t *data, uint32_t len)
{
    if (!Pmodbus_zdt_rs485_5678 || !Pmodbus_zdt_uart_5678 || !Pmodbus_zdt_rtu_master_5678 || !data)
        return -1;
    return Pmodbus_zdt_rs485_5678->rs485_send(data, len);
}

void screen_fresh_thread()
{
    while (!screen)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    tjc_screen::tjc_screen_page page;

    while (1)
    {
        screen->read_page(page);
        switch (page)
        {
        case tjc_screen::tjc_screen_page_status:
            screen->fresh_status_screen();
            break;
        case tjc_screen::tjc_screen_page_button:
            screen->detect_start_falg();
            // screen->change_page(tjc_screen::tjc_screen_page_number);
            break;
        case tjc_screen::tjc_screen_page_number:
            screen->fresh_number_screen();
            if (scanner)
                screen->set_number(scanner->get_code_str());
            break;

        case tjc_screen::tjc_screen_page_set_color:
            screen->fresh_set_color_sq_screen();
            break;
        default:
            break;
        }

        // LOG(INFO) << "page:" << (int)page;

        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
}

yj_return car_sensor_init()
{
    screen_uart = new uart_v2(TJC_SCREEN_PATH, TJC_SCREEN_BAUDRATE, 8, 1);
    screen = new tjc_screen(screen_uart);

    if (!screen || !screen_uart)
    {
        LOG(ERROR) << "tjc screen init failed";
        car_sensor_deinit();
        return YJ_ERROR;
    }
    screen->change_page(tjc_screen::tjc_screen_page_start);
    screen_fresh = std::thread(screen_fresh_thread);

    screen->set_status_item(tjc_screen::tjc_screen_status_next_page,
                            " ",
                            tjc_screen::tjc_screen_pic_big_error);

    pmw3901_spi = new spi_v1("/dev/spidev0.0", 2000000, spi_v1::spi_mode_3, 8);
    pmw3901_dev = new pmw3901(pmw3901_spi, "/dev/gpiochip3", 15);

    if (!pmw3901_dev || !pmw3901_spi)
    {
        LOG(ERROR) << "pmw3901 create failed";
        screen->set_status_item(tjc_screen::tjc_screen_status_spi,
                                ("pmw3901 init failed"),
                                tjc_screen::tjc_screen_pic_small_error);
        car_sensor_deinit();
        return YJ_ERROR;
    }

    if (pmw3901_dev->init() != YJ_OK)
    {
        LOG(INFO) << "pmw3901 init failed";
        screen->set_status_item(tjc_screen::tjc_screen_status_spi,
                                ("pmw3901 init failed"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }

    screen->set_status_item(tjc_screen::tjc_screen_status_spi,
                            ("pmw3901 init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    hwt101_i2c = new i2c_v1("/dev/i2c-8", 0x50);
    hwt101_dev = new hwt101("hwt101_test", hwt101_i2c, 0x50);

    if (!hwt101_dev || !hwt101_i2c)
    {
        LOG(ERROR) << "hwt101 create failed";
        screen->set_status_item(tjc_screen::tjc_screen_status_i2c,
                                ("hwt101 init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        car_sensor_deinit();
        return YJ_ERROR;
    }
    if (hwt101_dev->init() != YJ_OK)
    {
        LOG(ERROR) << "hwt101 init failed";
        screen->set_status_item(tjc_screen::tjc_screen_status_i2c,
                                ("hwt101 init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        car_sensor_deinit();
        return YJ_ERROR;
    }

    screen->set_status_item(tjc_screen::tjc_screen_status_i2c,
                            ("hwt101 init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    glob_t glob_result;
    if (glob("/dev/ttyACM*", GLOB_ERR, nullptr, &glob_result) != 0 || glob_result.gl_pathc == 0)
    {
        LOG(ERROR) << "未找到 /dev/ttyACM* 设备";
        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_5678,
                                ("code scanner init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        globfree(&glob_result);
        return YJ_ERROR;
    }

    std::string device_path = glob_result.gl_pathv[0];
    globfree(&glob_result);

    scanner_uart = new uart_v2(device_path, CODE_SCAN_BAUDRATE, 8, 1);
    scanner = new code_scan(scanner_uart);

    if (!scanner || !scanner_uart)
    {
        LOG(ERROR) << "scanner_uart init failed";
        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_5678,
                                ("code scanner init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        car_sensor_deinit();
        return YJ_ERROR;
    }
    screen->set_status_item(tjc_screen::tjc_screen_status_rs485_5678,
                            ("code scanner init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    return YJ_OK;
}

void car_sensor_deinit()
{
    if (pmw3901_dev)
    {
        delete pmw3901_dev;
        pmw3901_dev = nullptr;
    }

    if (pmw3901_spi)
    {
        delete pmw3901_spi;
        pmw3901_spi = nullptr;
    }

    if (hwt101_dev)
    {
        delete hwt101_dev;
        hwt101_dev = nullptr;
    }

    if (hwt101_i2c)
    {
        delete hwt101_i2c;
        hwt101_i2c = nullptr;
    }

    // if (screen)
    // {
    //     delete screen;
    //     screen = nullptr;
    // }

    // if (screen_uart)
    // {
    //     delete screen_uart;
    //     screen_uart = nullptr;
    // }

    if (scanner)
    {
        delete scanner;
        scanner = nullptr;
    }

    if (scanner_uart)
    {
        delete scanner_uart;
        scanner_uart = nullptr;
    }
}

yj_return car_init(void)
{
    if (car_sensor_init() != YJ_OK)
    {
        return YJ_ERROR;
    }
    LOG(INFO) << "car sensor init ok";

    std::string name;

    name = "car_wheel_12";
    Pmodbus_zdt_uart_12 = new uart_v2(ZDT_MOTOR_RS485_PATH_12, ZDT_MOTOR_RS485_BAUDRATE_12, 8, 1);
    Pmodbus_zdt_rs485_12 = new rs485_v3(Pmodbus_zdt_uart_12, ZDT_MOTOR_RS485_FRAME_TIMEOUT_MS);

    if (!Pmodbus_zdt_uart_12 || !Pmodbus_zdt_rs485_12)
    {
        if (Pmodbus_zdt_uart_12)
            delete Pmodbus_zdt_uart_12;
        if (Pmodbus_zdt_rs485_12)
            delete Pmodbus_zdt_rs485_12;

        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_12,
                                ("car wheel 12 init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }
    Pmodbus_zdt_rtu_master_12 = new agile_modbus_rtu_master_simple(name,
                                                                   modbus_zdt_rs485_modbus_write_12,
                                                                   modbus_zdt_rs485_modbus_read_12);

    if (!Pmodbus_zdt_rtu_master_12)
    {
        delete Pmodbus_zdt_uart_12;
        delete Pmodbus_zdt_rs485_12;

        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_12,
                                ("car wheel 12 init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }

    screen->set_status_item(tjc_screen::tjc_screen_status_rs485_12,
                            ("car wheel 12 init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    name = "car_wheel_34";
    Pmodbus_zdt_uart_34 = new uart_v2(ZDT_MOTOR_RS485_PATH_34, ZDT_MOTOR_RS485_BAUDRATE_34, 8, 1);
    Pmodbus_zdt_rs485_34 = new rs485_v3(Pmodbus_zdt_uart_34, ZDT_MOTOR_RS485_FRAME_TIMEOUT_MS);

    if (!Pmodbus_zdt_uart_34 || !Pmodbus_zdt_rs485_34)
    {
        if (Pmodbus_zdt_uart_34)
            delete Pmodbus_zdt_uart_34;
        if (Pmodbus_zdt_rs485_34)
            delete Pmodbus_zdt_rs485_34;
        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_12,
                                ("car wheel 12 ok 34 fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }

    Pmodbus_zdt_rtu_master_34 = new agile_modbus_rtu_master_simple(name,
                                                                   modbus_zdt_rs485_modbus_write_34,
                                                                   modbus_zdt_rs485_modbus_read_34);
    if (!Pmodbus_zdt_rtu_master_34)
    {
        delete Pmodbus_zdt_uart_34;
        delete Pmodbus_zdt_rs485_34;
        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_12,
                                ("car wheel 12 ok 34 fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }

    screen->set_status_item(tjc_screen::tjc_screen_status_rs485_12,
                            ("car wheel 1234 init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    name = "car_bracket_5678";
    Pmodbus_zdt_uart_5678 = new uart_v2(ZDT_MOTOR_RS485_PATH_5678, ZDT_MOTOR_RS485_BAUDRATE_5678, 8, 1);
    Pmodbus_zdt_rs485_5678 = new rs485_v3(Pmodbus_zdt_uart_5678, ZDT_MOTOR_RS485_FRAME_TIMEOUT_MS);

    if (!Pmodbus_zdt_uart_5678 || !Pmodbus_zdt_rs485_5678)
    {
        if (Pmodbus_zdt_uart_5678)
            delete Pmodbus_zdt_uart_5678;
        if (Pmodbus_zdt_rs485_5678)
            delete Pmodbus_zdt_rs485_5678;
        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_34,
                                ("claw init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }
    Pmodbus_zdt_rtu_master_5678 = new agile_modbus_rtu_master_simple(name,
                                                                     modbus_zdt_rs485_modbus_write_5678,
                                                                     modbus_zdt_rs485_modbus_read_5678);
    if (!Pmodbus_zdt_rtu_master_5678)
    {
        delete Pmodbus_zdt_uart_5678;
        delete Pmodbus_zdt_rs485_5678;
        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_34,
                                ("claw init fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }
    screen->set_status_item(tjc_screen::tjc_screen_status_rs485_34,
                            ("claw init ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    Pmodbus_wheel_1 = new zdt_motor(1, Pmodbus_zdt_rs485_12, Pmodbus_zdt_rtu_master_12);
    Pmodbus_wheel_2 = new zdt_motor(2, Pmodbus_zdt_rs485_12, Pmodbus_zdt_rtu_master_12);
    Pmodbus_wheel_3 = new zdt_motor(3, Pmodbus_zdt_rs485_34, Pmodbus_zdt_rtu_master_34);
    Pmodbus_wheel_4 = new zdt_motor(4, Pmodbus_zdt_rs485_34, Pmodbus_zdt_rtu_master_34);

    Pmodbus_rotate_5 = new zdt_motor(5, Pmodbus_zdt_rs485_5678, Pmodbus_zdt_rtu_master_5678);
    Pmodbus_lift_6 = new zdt_motor(6, Pmodbus_zdt_rs485_5678, Pmodbus_zdt_rtu_master_5678);
    Pmodbus_bracket_7 = new zdt_motor(7, Pmodbus_zdt_rs485_5678, Pmodbus_zdt_rtu_master_5678);
    Pmodbus_claw_board_8 = new pwm_board(8, Pmodbus_zdt_rs485_5678, Pmodbus_zdt_rtu_master_5678);

    if (!Pmodbus_wheel_1 || !Pmodbus_wheel_2 || !Pmodbus_wheel_3 || !Pmodbus_wheel_4 ||
        !Pmodbus_rotate_5 || !Pmodbus_lift_6 || !Pmodbus_bracket_7 || !Pmodbus_claw_board_8)
    {
        if (Pmodbus_wheel_1)
            delete Pmodbus_wheel_1;
        if (Pmodbus_wheel_2)
            delete Pmodbus_wheel_2;
        if (Pmodbus_wheel_3)
            delete Pmodbus_wheel_3;
        if (Pmodbus_wheel_4)
            delete Pmodbus_wheel_4;
        if (Pmodbus_rotate_5)
            delete Pmodbus_rotate_5;
        if (Pmodbus_lift_6)
            delete Pmodbus_lift_6;
        if (Pmodbus_bracket_7)
            delete Pmodbus_bracket_7;
        if (Pmodbus_claw_board_8)
            delete Pmodbus_claw_board_8;

        screen->set_status_item(tjc_screen::tjc_screen_status_rs485_34,
                                ("claw ok mdobus fail"),
                                tjc_screen::tjc_screen_pic_small_error);
        return YJ_ERROR;
    }

    Pcar = new car(Pmodbus_wheel_1, Pmodbus_wheel_2,
                   Pmodbus_wheel_3, Pmodbus_wheel_4,
                   Pmodbus_rotate_5, Pmodbus_lift_6, Pmodbus_bracket_7,
                   Pmodbus_claw_board_8,
                   85, 92, 75,
                   scanner, screen);

    if (!Pcar)
    {
        delete Pmodbus_zdt_uart_12;
        delete Pmodbus_zdt_rs485_12;
        delete Pmodbus_zdt_rtu_master_12;
        delete Pmodbus_zdt_uart_34;
        delete Pmodbus_zdt_rs485_34;
        delete Pmodbus_zdt_rtu_master_34;
        delete Pmodbus_zdt_uart_5678;
        delete Pmodbus_zdt_rs485_5678;
        delete Pmodbus_zdt_rtu_master_5678;
        delete Pmodbus_wheel_1;
        delete Pmodbus_wheel_2;
        delete Pmodbus_wheel_3;
        delete Pmodbus_wheel_4;
        delete Pmodbus_rotate_5;
        delete Pmodbus_lift_6;
        delete Pmodbus_bracket_7;
        return YJ_ERROR;
    }

    screen->set_status_item(tjc_screen::tjc_screen_status_rs485_34,
                            ("claw ok car ok"),
                            tjc_screen::tjc_screen_pic_small_ok);

    car_all_init_flag = YJ_OK;
    return YJ_OK;
}
